#include "cartographer/rcm/imu/preintegrate.h"

namespace cartographer
{
    /*
    https://zhuanlan.zhihu.com/p/511529274
    http://www.shouxieziti.cn/113163.html
    https://blog.csdn.net/qq_39266065/article/details/115348824
    积分项是相对于第i时刻的姿态，而不是相对于世界坐标系的姿态
    */

    void ImuPreintegrate::Initialize(const ImuBias &biasInit)
    {
        measures_.clear();
        sumTime_ = 0.0f;
        poseTime_ = 0;
        hasIntegrated = false;

        dR.setIdentity();
        dV.setZero();
        dP.setZero();
        // 偏置的雅可比
        JRg.setZero();
        JVg.setZero();
        JVa.setZero();
        JPg.setZero();
        JPa.setZero();

        biasOrg_ = biasInit;
        biasNew_ = biasInit;
        biasDelta_.setZero();
        convariance_.setZero();
        jacobian_.setIdentity();
    }

    // 噪声传递，预积分更新，雅可比更新都是用递推形式表示
    void ImuPreintegrate::Update(const Eigen::Vector3d &acceleration, const Eigen::Vector3d &angularVel, const double &dt)
    {
        measures_.emplace_back(acceleration, angularVel, dt);
        /*
         * 第一部分：噪声传递方程
         * 参考 https://zhuanlan.zhihu.com/p/338919099
         * 随时间变化的噪声递推方程，噪声包含两部分:
         * 1) 当前时刻预积分量噪声传递给下一时刻
         * 2) 当前时刻测量噪声传递给下一时刻
         * 注意预积分量的计算是没有考虑噪声的，求噪声的协方差是为了度量预积分量的不确定性
         *
         * 噪声随时间变化的导数关系: δx˙= Aδx + Bn
         * 噪声传递方程: δx(k) = δx(k−1) +δx˙(k−1)∆t 即 δx(k) = (I + A∆t)δx(k−1) + B∆tn(k−1)
         */

        /*
         * A为预积分量噪声的传递矩阵，噪声向量=[姿态R噪声,速度V噪声，位移P噪声]，每项三维则共9维；
         * 注意网络博客中噪声向量和预积分向量中的顺序是不同的(博客都不会指明)
         */
        Eigen::Matrix<double, 9, 9> A;
        A.setIdentity();
        /*
         * B为测量噪声的传递矩阵，噪声向量=[陀螺仪噪声,加速噪声]，共6维度；使用AB矩阵将两部分的噪声分离；
         */
        Eigen::Matrix<double, 9, 6> B;
        B.setZero();
        // 这两个传递矩阵的数都是当前时刻的，用其计算下一时刻的矩阵
        Eigen::Vector3d acc = acceleration - biasOrg_.acc_;
        Eigen::Vector3d angular = angularVel - biasOrg_.gyro_;
        // 第二部分：离散形式的预积分量计算公式的递推形式
        // 基于上一时刻的旋转更新当前时刻IMU的累计位移和速度
        // 更新预积分量中的位移项,dR将加速度转到世界系
        dP = dP + dV * dt + 0.5f * dR * acc * dt * dt;
        // 更新预积分量中的速度项
        dV = dV + dR * acc * dt;
        // 将加速度向量转为反对称矩阵
        Eigen::Matrix3d acc_w = Sophus::SO3d::hat(acc);

        /*
         * 位移公式包含速度与姿态，故位移噪声受速度和姿态噪声影响；速度公式包含姿态，故速度噪声受姿态噪声影响；
         * 所以这里求三个偏导，除姿态外，位移噪声和速度噪声对自身的偏导都是单位矩阵；
         */
        // 速度噪声传递方程中对姿态R噪声的偏导
        A.block<3, 3>(3, 0) = -dR * dt * acc_w;
        // 位移噪声传递方程中对姿态R噪声的偏导
        A.block<3, 3>(6, 0) = -0.5f * dR * dt * dt * acc_w;
        // 位移噪声传递方程中对速度噪声的偏导，偏导就是dt
        A.block<3, 3>(6, 3) = Eigen::DiagonalMatrix<double, 3>(dt, dt, dt);
        // 速度噪声传递方程中对陀螺仪噪声的偏导
        B.block<3, 3>(3, 3) = dR * dt;
        // 位移噪声传递方程中对加速噪声的偏导
        B.block<3, 3>(6, 3) = 0.5f * dR * dt * dt;
        // Update position and velocity jacobians wrt bias correction
        // 因为随着时间推移，不可能每次都重新计算雅克比矩阵，所以需要做J(k+1) = j(k) + (~)这类事，分解方式与AB矩阵相同
        // 论文作者对forster论文公式的基础上做了变形，然后递归更新。
        // 参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212

        // 第三部分：定义偏置的雅可比方程的递推形式
        // 位移与加速偏置的雅可比
        JPa = JPa + JVa * dt - 0.5f * dR * dt * dt;
        // 位移与陀螺仪偏置的雅可比
        JPg = JPg + JVg * dt - 0.5f * dR * dt * dt * acc_w * JRg;
        // 线速与加速偏置的雅可比
        JVa = JVa - dR * dt;
        // 线速与陀螺仪速偏置的雅可比
        JVg = JVg - dR * dt * acc_w * JRg;
        // Update delta rotation
        IntegratedRotation dRi(angular, dt);
        // 更新旋转
        dR = NormalizeRotation(dR * dRi.deltaR);
        // 姿态噪声传递方程中对姿态噪声的偏导(对自身的偏导)
        A.block<3, 3>(0, 0) = dRi.deltaR.transpose();
        // 姿态噪声传递方程中对陀螺仪噪声的偏导
        B.block<3, 3>(0, 0) = dRi.rightJ * dt;
        // 协方差对应的噪声依次是姿态，速度，位移，陀螺仪偏置，加速偏置
        convariance_.block<9, 9>(0, 0) = A * convariance_.block<9, 9>(0, 0) * A.transpose() + B * Nga * B.transpose();
        // 陀螺仪和加速对自身的不确定性就是随机游走
        convariance_.block<6, 6>(9, 9) += NgaWalk;
        JRg = dRi.deltaR.transpose() * JRg - dRi.rightJ * dt;
        SetJacobianMatrix();
        // PrintMatrix("imupre_cov", convariance_);
        sumTime_ += dt;
        cnt++;
    }

    void ImuPreintegrate::Reintegrate()
    {
        std::unique_lock<std::mutex> lock(mutex_);
        const auto aux = measures_;
        Initialize(biasNew_);
        for (size_t i = 0; i < aux.size(); i++)
            Update(aux[i].acceleration, aux[i].angular, aux[i].dt);
    }

    void ImuPreintegrate::SetJacobianMatrix()
    {
        jacobian_.setIdentity();
        jacobian_.block<3, 3>(OFF_P, OFF_BA) = JPa;
        jacobian_.block<3, 3>(OFF_P, OFF_BG) = JPg;
        jacobian_.block<3, 3>(OFF_R, OFF_BG) = JRg;
        jacobian_.block<3, 3>(OFF_V, OFF_BA) = JVa;
        jacobian_.block<3, 3>(OFF_V, OFF_BG) = JVg;
    }

    void ImuPreintegrate::SetNewBias(const ImuBias &bias)
    {
        std::unique_lock<std::mutex> lock(mutex_);
        biasNew_ = bias;
        biasDelta_.block<3, 1>(3, 0) = biasNew_.acc_ - biasOrg_.acc_;
        biasDelta_.block<3, 1>(0, 0) = biasNew_.gyro_ - biasOrg_.gyro_;
    }
} // namespace cartographer